
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srvrly_events.c
  * @author     baiyang
  * @date       2023-2-28
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "srvrly_events.h"

#include <relay/relay.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <rc_channel/rc_channel.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
struct servo_relay_events srvrly_events;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
bool srvrly_do_set_servo(uint8_t channel, uint16_t pwm)
{
    srv_channel *c = srv_channels_get_channel(channel-1);
    if (c == NULL) {
        return false;
    }
    switch(srv_channel_get_function(c))
    {
    case k_none:
    case k_manual:
    case k_sprayer_pump:
    case k_sprayer_spinner:
    case k_gripper:
    case k_rcin1: // rc pass-thru
    case k_rcin2:
    case k_rcin3:
    case k_rcin4:
    case k_rcin5:
    case k_rcin6:
    case k_rcin7:
    case k_rcin8:
    case k_rcin9:
    case k_rcin10:
    case k_rcin11:
    case k_rcin12:
    case k_rcin13:
    case k_rcin14:
    case k_rcin15:
    case k_rcin16:
        break;
    default:
        gcs_send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", channel);
        return false;
    }
    if (srvrly_events.type == SRVRLY_EVENT_TYPE_SERVO && 
        srvrly_events.channel == channel) {
        // cancel previous repeat
        srvrly_events.repeat = 0;
    }
    srv_channel_set_output_pwm(c, pwm, false);
    srv_channel_ignore_small_rcin_changes(c);
    return true;
}

#if MB_RELAY_ENABLED
bool srvrly_do_set_relay(uint8_t relay_num, uint8_t state)
{
    if (!rly_enabled(relay_num)) {
        return false;
    }
    if (srvrly_events.type == SRVRLY_EVENT_TYPE_RELAY && 
        srvrly_events.channel == relay_num) {
        // cancel previous repeat
        srvrly_events.repeat = 0;
    }
    if (state == 1) {
        rly_on(relay_num);
    } else if (state == 0) {
        rly_off(relay_num);
    } else {
        rly_toggle(relay_num);
    }
    return true;
}
#endif

bool srvrly_do_repeat_servo(uint8_t _channel, uint16_t _servo_value, 
                                          int16_t _repeat, uint16_t _delay_ms)
{
    srv_channel *c = srv_channels_get_channel(_channel-1);
    if (c == NULL) {
        return false;
    }
    switch(srv_channel_get_function(c))
    {
    case k_none:
    case k_manual:
    case k_sprayer_pump:
    case k_sprayer_spinner:
    case k_gripper:
    case k_rcin1: // rc pass-thru
    case k_rcin2:
    case k_rcin3:
    case k_rcin4:
    case k_rcin5:
    case k_rcin6:
    case k_rcin7:
    case k_rcin8:
    case k_rcin9:
    case k_rcin10:
    case k_rcin11:
    case k_rcin12:
    case k_rcin13:
    case k_rcin14:
    case k_rcin15:
    case k_rcin16:
        break;
    default:
        gcs_send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
        return false;
    }
    srvrly_events.channel = _channel;
    srvrly_events.type = SRVRLY_EVENT_TYPE_SERVO;

    srvrly_events.start_time_ms  = 0;
    srvrly_events.delay_ms    = _delay_ms / 2;
    srvrly_events.repeat      = _repeat * 2;
    srvrly_events.servo_value = _servo_value;
    srvrly_update_events();
    return true;
}

#if MB_RELAY_ENABLED
bool srvrly_do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
{
    if (!rly_enabled(relay_num)) {
        return false;
    }
    srvrly_events.type = SRVRLY_EVENT_TYPE_RELAY;
    srvrly_events.channel = relay_num;
    srvrly_events.start_time_ms  = 0;
    srvrly_events.delay_ms        = _delay_ms/2; // half cycle time
    srvrly_events.repeat          = _repeat*2;  // number of full cycles
    srvrly_update_events();
    return true;
}
#endif

  /*
    update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
  */
void srvrly_update_events(void)
{
    if (srvrly_events.repeat == 0 || (time_millis() - srvrly_events.start_time_ms) < srvrly_events.delay_ms) {
        return;
    }

    srvrly_events.start_time_ms = time_millis();

    switch (srvrly_events.type) {
    case SRVRLY_EVENT_TYPE_SERVO: {
        srv_channel *c = srv_channels_get_channel(srvrly_events.channel-1);
        if (c != NULL) {
            if (srvrly_events.repeat & 1) {
                srv_channel_set_output_pwm(c, srv_channel_get_trim(c), false);
            } else {
                srv_channel_set_output_pwm(c, srvrly_events.servo_value, false);
                srv_channel_ignore_small_rcin_changes(c);
            }
        }
        break;
    }

#if MB_RELAY_ENABLED
    case SRVRLY_EVENT_TYPE_RELAY: {
            rly_toggle(srvrly_events.channel);
        break;
    }
#endif
    default:
        break;
    }

    if (srvrly_events.repeat > 0) {
        srvrly_events.repeat--;
    } else {
        // toggle bottom bit so servos flip in value
        srvrly_events.repeat ^= 1;
    }
}

/*------------------------------------test------------------------------------*/


